function [x, P, K] = kalmfiltupd_joseph(x, P, zObs, H, GammaMRGammaMT, fcnMTimes) %#codegen
%KALMFILTUPD_JOSEPH 卡尔曼滤波增益计算、状态估计更新及误差协方差更新，采用约瑟夫型误差协方差更新计算式
%
% Input Arguments:
% # x: m*1列向量，状态估计更新之前的状态向量
% # P: m*m矩阵，误差协方差更新之前的误差协方差矩阵
% # zObs: n*1列向量，观测量
% # H: n*m矩阵，量测矩阵
% # GammaMRGammaMT: n*n矩阵,等价于GammaM*R*GammaMT，其中GammaM为量测噪声耦合矩阵，R为量测噪声序列协方差矩阵，GammaMT为GammaM的转置
% # fcnMTimes: 函数句柄，用于计算本函数中的矩阵乘法，默认为普通乘法
%
% Output Arguments:
% # x: m*1列向量，状态估计更新之后的状态向量
% # P: m*m矩阵，误差协方差更新之后的误差协方差矩阵
% # K: m*n矩阵,卡尔曼滤波增益矩阵
%
% References:
% # 《应用导航算法工程基础》“滤波增益矩阵的计算”

if (nargin<6) || isempty(fcnMTimes)
    fcnMTimes = @kfupddefaultmtimes;
end
K = fcnMTimes(int8(KFUpdMTimesFcnType.PHT), P, H') / (fcnMTimes(int8(KFUpdMTimesFcnType.HPHT), H, P) + GammaMRGammaMT); % REF1式12.46
x = x + K*(zObs - fcnMTimes(int8(KFUpdMTimesFcnType.Hx), H, x)); % REF1式12.18、式12.19
M = eye(size(P)) - fcnMTimes(int8(KFUpdMTimesFcnType.KH), K, H);
% 分解成两步计算
tmp_MP = fcnMTimes(int8(KFUpdMTimesFcnType.MP), M, P);
P = fcnMTimes(int8(KFUpdMTimesFcnType.MPMT), tmp_MP, M') + fcnMTimes(int8(KFUpdMTimesFcnType.KGammaMRGammaMTKT), K, GammaMRGammaMT);
end